#include <WProgram.h>
#include "Streaming/Streaming.h"
#include "NewSoftSerial/NewSoftSerial.h"
#include "TinyGPS/TinyGPS.h"
#include "Commands/Commands.h"
#include "XBee2/XBee2.h"

#define GPS_BIT_RATE 4800
#define SERIAL_BIT_RATE 19200
#define GPS_SERIAL_PINS 2,3

#define GPS_VCC_PIN 4

struct Location {
  long latitude;
  long longitude;
};


unsigned long lastCommand = 0;

XBee2 xbee;
NewSoftSerial gpsSerial(GPS_SERIAL_PINS);
TinyGPS gps;

#ifndef _GPS_NO_STATS
unsigned long chars;
unsigned short sentences;
unsigned short failed_cs;
unsigned short old_failed_cs;
#endif

unsigned long speed = 0;
unsigned int course = 0;

struct Location current = { 0, 0 };
struct Location target = { 0, 0 };

void feedgps();
void readGps();
int bearing(float currentLatitude, float currentLongitude, float targetLatitude, float targetLongitude);
int distance(long currentLatitude, long currentLongitude, long targetLatitude, long targetLongitude);
int bearingToTarget();
int distanceToTarget();
float toRadians(long degrees);
void outputStatus();
void reportGpsStats();
void commandDriver();

void handleNavigatorCommand(char command);
void loop();
void powerUp();
void main() __attribute__ ((noreturn));

void main(void) {
  init();
  powerUp();
  gpsSerial.begin(GPS_BIT_RATE);
  Serial.begin(SERIAL_BIT_RATE);
  xbee.enterApiMode(false);
  xbee >> COORDINATOR_ADDRESS << "N. reset" << endl;
  for (;;) {
    loop();
  }
}

void loop() {
  feedgps();
  xbee.receiveStuff(&handleNavigatorCommand);
  commandDriver();
}

void feedgps() {
  do {
    if (gps.encode(gpsSerial.read())) {
      readGps();
    }
  } while (gpsSerial.available());
}

void readGps() {
  speed = gps.speed() * _GPS_KMPH_PER_KNOT;
  course = gps.course() / 10;
  gps.get_position(&current.latitude, &current.longitude);
}

#ifndef _GPS_NO_STATS
void reportGpsStats() {
  gps.stats(&chars, &sentences, &failed_cs);
  if (failed_cs > old_failed_cs) {
    old_failed_cs = failed_cs;
    xbee >> COORDINATOR_ADDRESS << "c: " << chars << " s: " << sentences << " e: " << failed_cs << endl;
  }
}
#endif

extern "C" void __cxa_pure_virtual() {
  cli();
  for (;;)
    ;
}

void handleNavigatorCommand(char command) {
  switch (command) {
  case C_TARGET:
    target.latitude = current.latitude;
    target.longitude = current.longitude;
    xbee >> COORDINATOR_ADDRESS << "t.a." << endl;
    break;
  case C_PRINT:
    outputStatus();
    break;
  case C_GO_TO_API_MODE:
    xbee.enterApiMode(true);
    break;
  case C_GO_TO_REGULAR_MODE:
    xbee.enterApiMode(false);
    break;
  default:
    break;
  }
}

int bearing(float currentLatitude, float currentLongitude, float targetLatitude, float targetLongitude) {
  float deltaY = sin(targetLongitude - currentLongitude);
  float sinCurrentLatitude = sin(currentLatitude);
  float cosCurrentLatitude = cos(currentLatitude);
  float cosTargetLatitude = cos(targetLatitude);
  float sinTargetLatitude = sin(targetLatitude);

  float deltaX = sinTargetLatitude * cosCurrentLatitude / cosTargetLatitude - sinCurrentLatitude;

  float bearing = atan2(deltaY, deltaX);
  bearing = bearing * 1800 / PI;
  int result = (int) bearing;
  return result + 3600;
}

int distance(long currentLatitude, long currentLongitude, long targetLatitude, long targetLongitude) {
  return abs(currentLatitude - targetLatitude) + abs(currentLongitude - targetLongitude);
}


float toRadians(long degrees) {
  return degrees * PI / 18000000;
}

int bearingToTarget() {
  return bearing(toRadians(current.latitude), toRadians(current.longitude), toRadians(target.latitude), toRadians(target.longitude));
}

int distanceToTarget() {
  return distance(current.latitude, current.longitude, target.latitude, target.longitude);
}

void outputStatus() {
  xbee >> COORDINATOR_ADDRESS << "s: " << speed;
  xbee << " c: " << course;
  xbee << " lat: " << current.latitude;
  xbee << " lon: " << current.longitude;
  int _bearing = bearingToTarget();
  xbee << " b: " << _bearing;
  int _distance = distanceToTarget();
  xbee << " d: " << _distance << endl;
}


void commandDriver() {
  if (xbee.inApiMode()) {
      xbee >> DRIVER_ADDRESS << bearingToTarget() << distanceToTarget();
    }
}

void powerUp() {
  pinMode(GPS_VCC_PIN, OUTPUT);
  digitalWrite(GPS_VCC_PIN, HIGH);
}
